/**
*/


#include <OGF/quick_start/common/simplify.h>
#include <OGF/quick_start/common/select_util.h>
#include <OGF/basic/os/stopwatch.h>
#include <OGF/cells/map/map_cell_heap.h>

#include <set>
#include <stack>
#include <queue>
#include <limits>

#include <OGF/math/linear_algebra/vector.h>
#include <OGF/quick_start/common/refinement.h>

using namespace std;

namespace OGF 
{
#ifdef _WINDOWS
	//HACK
	GraphCombelObserver<Graph::Halfedge>::GraphCombelObserver(Graph* m) : graph_(m) {
        graph_->add_halfedge_observer(this);
    }
    
    GraphCombelObserver<Graph::Halfedge>::~GraphCombelObserver() {
        graph_->remove_halfedge_observer(this);
    }
	//SLASH HACK
#endif

	Simplify::Simplify()
	{
	}
	Simplify::~Simplify()
	{

	}

	/**
	* Do init, but only if the surface has changed.
	* Checks is the surface has changed by pointer equality,
	* and number of verts and half-edges.
	*/
	void Simplify::lazy_init(Line *graph, Surface *surface)
	{	
		//Properties we watch on the mesh:
		static int num_verts = -1;
		static int num_edge = -1;

		//if the surface has changed since the last time we ran init(),
		//then run it again.
		if(surface != this->surface 
			|| num_verts != surface->size_of_vertices() 
			|| num_edge != surface->size_of_halfedges())
		{
			num_verts = surface->size_of_vertices();
			num_edge = surface->size_of_halfedges();

			init(graph, surface); 
		}
	}

	void Simplify::init(Line *graph, Surface *s)
	{
		this->surface = s;
		this->skeleton = graph;

		vQ = new GraphVertexAttribute<Matrix4d >(skeleton, "vQ" );
		edgeError = new GraphHalfedgeAttribute<double>(skeleton, "edgeError" );

		// Calculate the error metric matrix for each vertex
		FOR_EACH_VERTEX ( Graph, skeleton, it ) {
			initializeVertexError(it);
		}

		// Set the error associated with collapsing each edge
		FOR_EACH_HALFEDGE ( Graph, skeleton, it ) {
			double error = getCollapseError ( it, getCollapsePoint(it) );
			(*edgeError)[it] = error;
		}



	}
	
	/*
	 * Returns true if this surface has been initialized for the simplifier
	 * false otherwise
	 */
	bool Simplify::isInitialized (){
		return GraphVertexAttribute<Matrix4d>::is_defined( skeleton, "vQ" );
	}	

	void Simplify::simplifyNext(GraphHalfedgeHeap *heap) {
		
		Graph::Halfedge* he = heap->pop();
		Graph::Vertex* v = he->vertex();
		// Collapse the edge
		collapseEdge(he);

		updateLocalVertexErrors(v, heap);

	}

	/*
 	 * Simplify the mesh by collapsing a single edge
 	 */
	void Simplify::simplifyMeshSingle() {

		if(!isInitialized()){
			std::cout << "Simplifier isn't initialized. Initialize it first." << std::endl;
			return;
		}

		// Initialize a halfedge heap sorted in terms of collapse error
		GraphHalfedgeHeap heap(skeleton, *edgeError);
		FOR_EACH_EDGE ( Graph, skeleton, it ){
			heap.push(it);
		}

		simplifyNext(&heap);


	}


	/*
	 * Simplify the mesh until we reach a user supplied minimum vertex count
	 */
	void Simplify::simplifyMesh ( int vertex_count ) {

		if(!isInitialized()){
			std::cout << "Simplifier isn't initialized. Initialize it first." << std::endl;
			return;
		}

		// Initialize a halfedge heap sorted in terms of collapse error
		GraphHalfedgeHeap heap(skeleton, *edgeError);
		FOR_EACH_EDGE ( Graph, skeleton, it ){
			heap.push(it);
		}

		while ( skeleton->size_of_vertices() > vertex_count && !heap.empty()) {
			simplifyNext(&heap);
		}

	}

	Point3d Simplify::getCollapsePoint(Graph::Halfedge* edge){
		/*Vector3d p1 = edge->vertex()->point() - Point3d(0,0,0);
		Vector3d p2 = edge->opposite()->vertex()->point() - Point3d(0,0,0);
		Vector3d p = (p1 + p2)/2.;
		return Point3d(p[0],p[1],p[2]);*/
		return edge->opposite()->vertex()->point();
	}

	/*
	* Collapse the next edge in the queue
	*/
	void Simplify::collapseEdge(Graph::Halfedge* edge){

		GraphVertexAttribute<vector<Map::Vertex*> > mapVertices(skeleton, "mapVertices");
		MapVertexAttribute<Graph::Vertex*> skeletonVertex(surface, "skeletonVertex");

		//Refinement Variables
		GraphVertexAttribute<VertexSet> refineAssociatedVerts(skeleton, "ASSOCIATED_VERTS");

		GraphEditor editor(skeleton);
		
		if(edge != nil && edge->opposite() != nil && edge->opposite()->vertex() != nil && edge->vertex() != nil){
			
			Graph::Vertex* deletedVertex = edge->opposite()->vertex();
			Graph::Vertex* remainingVertex = edge->vertex();

			// Update Skeleton-Map Correspondance
			// 	add the map vertex corresponding to the removed skeleton vertex to the 
			// 	remaining skeleton vertex's list of corresponding vertices
			for(unsigned int i = 0 ; i < mapVertices[deletedVertex].size(); i++){
				skeletonVertex[mapVertices[deletedVertex][i] ] = remainingVertex;
				mapVertices[remainingVertex].push_back(mapVertices[deletedVertex][i]);

				refineAssociatedVerts[remainingVertex].insert(mapVertices[deletedVertex][i]);
			}
			mapVertices[deletedVertex].clear();

			// Update the vertex error metric
			(*vQ)[remainingVertex] = (*vQ)[remainingVertex] + (*vQ)[deletedVertex]; 
			
			// Merge the two graph vertices
			remainingVertex->set_point(getCollapsePoint(edge));
			editor.merge_vertices(remainingVertex, deletedVertex); // 2nd vertex param is erased

		}

	
	}

	void printmatrix(Matrix4d A){
		printf("\n");
		for(int i = 0 ; i < 4 ; i++){
			for(int j=0 ; j<4 ; j++){
				printf("%f ", A(i,j));
			}
			printf("\n");
		}
		printf("\n");
		fflush(stdout);
			
	}

	double Simplify::getCollapseError( Graph::Halfedge* he, Point3d p ) {

		// May crash on endpoints
		Matrix4d Q = (*vQ)[he->vertex()] + (*vQ)[he->opposite()->vertex()];

		double totalError = 0.;
		double wshape = 1.;
		//double wsample = 0.1;
		double wsample = 0.1;
		//double wsample = 0.1;

		// SHAPE COST
		// Multiply v'*Q*v
		// remember that v is in homogeneous coordinates
		double v[4];
		double Qv[4];
		v[0] = p.x(); v[1] = p.y(); v[2] = p.z(); v[3] = 1.;
		mult ( Q, v, Qv );
		double shapeError = Qv[0]*v[0] + Qv[1]*v[1] + Qv[2]*v[2] + Qv[3]*v[3];
		
		totalError += wshape * shapeError;
	
		// SAMPLING COST
		double sampleError = getEdgeSamplingCost(he);
		//cout << " sampleError : " << sampleError << " shapecost " << shapeError << endl;
		totalError += wsample * sampleError;

		// Extremity Penalty
		if(he->vertex()->is_extremity() || he->opposite()->vertex()->is_extremity())
			totalError *= 1000.;

		// Penalize High Degree Vertices
		//double degreepenalty = 1.;
		//if(he->vertex()->degree() ==3 || he->opposite()->vertex()->degree() == 3){
		//	degreepenalty = 50.;
		//}
		//if(he->vertex()->degree() > 2 || he->opposite()->vertex()->degree() > 2){
			//degreepenalty = 100000000.;
		//}
		//totalError *= degreepenalty;
		
		// remove
		//totalError = p[1];

		return totalError;
	}


	/*
	 * Update the error metrics for the neighbouring vertices
	 */
	void Simplify::updateLocalVertexErrors(Graph::Vertex* v, GraphHalfedgeHeap *heap){

		// Update the error for each halfedge surrounding this vertex
		// Update the cost for this collapse in the heap

		// Loop over in edges
		Graph::Halfedge* he = v->halfedge();
		do {
			if(he == nil || he->opposite() == nil)
				break;
			(*edgeError)[he] = getCollapseError( he, getCollapsePoint(he));
			(*edgeError)[he->opposite()] = getCollapseError( he->opposite(), getCollapsePoint(he->opposite()));

			if(heap->contains(he))
				heap->update_cost(he);
			else
				heap->push(he);
			if(heap->contains(he->opposite()))
				heap->update_cost(he->opposite());
			else
				heap->push(he->opposite());					

			he = he->prev_around_vertex();
		} while ( he != v->halfedge() );

	}


	void Simplify::initializeVertexError(Graph::Vertex *v){
		Matrix4d Q;
		Matrix4d K;
		Matrix4d Kt;

		// loop around the edges around this vertex and add up the K matrices
		Q.load_zero();
		Graph::Halfedge *he = v->halfedge();
	
		do {
			K = getEdgeMetric( he );
			Kt = K.transpose();
			Q = Q + (Kt*K);
			he = he->prev_around_vertex();

		} while ( he != v->halfedge() );

		(*vQ)[v] = Q;
	}


	Matrix4d Simplify::getEdgeMetric (Graph::Halfedge *he ){

		// Kij =
		//	[	0	-az	ay	-bx ]
		//	[	az	0	-ax	-by ]
		//	[	-ay	ax	0	-bz ]
		//
		// a = normalized edge vector
		// b = a x vi

		Vector3d a = he->vertex()->point() - he->opposite()->vertex()->point();
		a.normalize();
		Vector3d vi = he->vertex()->point() - Point3d(0.,0.,0.);
		Vector3d b = (a)^vi;

		Matrix4d K;

		K ( 0, 0 ) = 0.0;	K ( 0, 1 ) = -a.z();	K ( 0, 2 ) = a.y();	K ( 0, 3 ) = -b.x();
		K ( 1, 0 ) = a.z();	K ( 1, 1 ) = 0.0;	K ( 1, 2 ) = -a.x();	K ( 1, 3 ) = -b.y();
		K ( 2, 0 ) = -a.y();	K ( 2, 1 ) = a.x();	K ( 2, 2 ) = 0.0;	K ( 2, 3 ) = -b.z();
		K ( 3, 0 ) = 0.0;	K ( 3, 1 ) = 0.0;	K ( 3, 2 ) = 0.0;	K ( 3, 3 ) = 0.0;

		return K;

	}

	/* 
	 * Find the sampling cost for this halfedge
	 */
	double Simplify::getEdgeSamplingCost (Graph::Halfedge *he ){
		

		Vector3d v = he->vertex()->point() - he->opposite()->vertex()->point();
		double dij = v.norm();
		
		double dtot = 0;

		Graph::Vertex* vert = he->vertex();
		do{
			v = he->vertex()->point() - he->opposite()->vertex()->point();
			dtot += v.norm();

			he = he->prev_around_vertex();
		} while ( he != vert->halfedge() );

		return dij*dtot;

	}




} //namespace
